//
// Created by Shin on 2023/4/30.
//
#include "RTOS_tasks.h"
#include "Peripheral.h"
#include "calc.h"
#include "Drv_Screen.h"
#include "Drv_UpperComputer.h"
#include "PWM.h"
#include "tim.h"
#include "servo_pid.h"
#include "../Peripheral.h"
#include "../PWM.h"

int start_mission = 0;
void control_task(void const * argument)
{

    while(1)
    {
//        if(!isConnect)
//        {
//            car_stop(&mycar);
//            osDelay(5);
//            continue;
//        }

        Get_Eulerangle(&(mycar.now_pitch),
                        &(mycar.now_roll),
                        &(mycar.now_yaw));

        Get_now_xy(&(mycar.now_dis_x),
                   &(mycar.now_dis_y));
#if !NC_control
        SDK_run(&mycar);

        if(mycar.input_cfg == INPUT_POSITION)
        {
            mycar.delta_dis = Get_Euclidean_Distance(mycar.target_dis_x - mycar.now_dis_x,
                                                     mycar.target_dis_y - mycar.now_dis_y);
            mycar.target_yaw = Get_angle(mycar.now_dis_x, mycar.now_dis_y,
                                         mycar.target_dis_x, mycar.target_dis_y);
        }
        else if(mycar.input_cfg == INPUT_ANG_DIS)
        {
            //Get_barrier(&(mycar.delta_dis), &(mycar.target_yaw));
        }

        mycar.target_yaw -= mycar.keep_angle;
        mycar.delta_dis -= mycar.keep_dis;

#endif
        if(car_is_arrive(&mycar, mycar.delta_dis) && !NC_control && (0 == get_residual_light_num(0)))
        {
            SDK_finish();
            car_stop(&mycar);
        }
        else
        {
            car_update(&mycar);
        }
        //osDelay(3);
        osDelay(10);
    }
}
void StartU1Task(void const * argument) //树莓派
{
    int sta = 0;
    int time_cnt=0;
    for(;;)
    {
        if(uart[0].state != sta)
        {
            time_cnt = 0;
            sta = uart[0].state;
            //TODO
            UpperComputer_DataAnl(uart[0].date);
        }
        else
        {
            if(time_cnt >= 1000)
            {
                //TODO:断联处理函数
            }
            else
            {
                time_cnt += 10;
            }
        }
        //HAL_GPIO_TogglePin(LED_W_GPIO_Port, LED_W_Pin);
        osDelay(500);
    }
}
void StartU2Task(void const * argument)  //imu
{
    int sta = 0;
    int time_cnt=0;
    for(;;)
    {
        if(uart[1].state != sta)
        {
            time_cnt = 0;
            sta = uart[1].state;
            //TODO
        }
        else
        {
            if(time_cnt >= 1000)
            {
                //TODO:断联处理函数
            }
            else
            {
                time_cnt += 10;
            }
        }
        osDelay(10);
    }
}
void StartU4Task(void const * argument) //飞机
{
    int sta = 0;
    int time_cnt=0;
    for(;;)
    {
        if(uart[2].state != sta)
        {
            time_cnt = 0;
            sta = uart[2].state;
            //TODO
            Uav_DataAnl(uart[2].date);
        }
        else
        {
            if(time_cnt >= 1000)
            {
                //TODO:断联处理函数
            }
            else
            {
                time_cnt += 10;
            }
        }
        osDelay(10);
    }
}
void StartU5Task(void const * argument) //串口屏
{
    int sta = 0;
    int num=0;
    int time_cnt=0;
    for(;;)
    {
        if(uart[3].state != sta)
        {
            time_cnt = 0;
            sta = uart[3].state;
            //TODO
            Screen_DataAnl(uart[3].date);
            HAL_GPIO_WritePin(LED_W_GPIO_Port, LED_R_Pin,GPIO_PIN_SET);
        }
        else
        {
            if(time_cnt >= 1000)
            {
                //TODO:断联处理函数
                time_cnt=0;
            }
            else
            {
                time_cnt += 10;
            }
        }
        //Screen_task();
        //HAL_GPIO_TogglePin(LED_W_GPIO_Port, LED_W_Pin);
        osDelay(20);
    }
}
void StartU6Task(void const * argument) //v831
{
    int sta = 0;
    int time_cnt=0;
    for(;;)
    {
        if(uart[4].state != sta)
        {
            time_cnt = 0;
            sta = uart[4].state;
            v831_DataAnl(uart[4].date);
            //TODO
        }
        else
        {
            if(time_cnt >= 1000)
            {
                //TODO:断联处理函数
                time_cnt=0;
            }
            else
            {
                time_cnt += 10;
            }
        }
        osDelay(20);
    }
}
extern pwm_s PWM_S;
void SDK_Run_Task(void const * argument)
{
    for(;;)
    {
        Get_Eulerangle(&(mycar.now_pitch),
                       &(mycar.now_roll),
                       &(mycar.now_yaw));

        Get_now_xy(&(mycar.now_dis_x),
                   &(mycar.now_dis_y));
        if(start_mission)
        {
            SDK_run(&mycar);
            mycar.delta_dis = Get_Euclidean_Distance(mycar.target_dis_x - mycar.now_dis_x,
                                                     mycar.target_dis_y - mycar.now_dis_y);
            mycar.target_yaw = Get_angle(mycar.now_dis_x, mycar.now_dis_y,
                                         mycar.target_dis_x, mycar.target_dis_y);
            if (car_is_arrive(&mycar, mycar.delta_dis) && (0 == get_residual_light_num(0))) {
                SDK_finish();
                car_stop(&mycar);
            } else {
                car_update(&mycar);
            }
        }
        osDelay(3);
    }
}
extern servo_pid_obj servo_pid_1;
void Ser_Handle_Task(void const * argument)
{
    servo_pid_myinit(&servo_pid_1);
    servo_pid_1.sta=1;
    for(;;)
    {
        servo_pid_update(&servo_pid_1);
        osDelay(500);
    }
}
void Screen_Hanle_Task(void const * argument)
{
    int sta = 0;
    int time_cnt=0;
    for(;;)
    {
        Screen_task();
        osDelay(1000);
    }
}

extern servo_pid_obj servo_pid_1;
void Vel_Crt_Handle(void const * argument)
{
//    servo_pid_myinit(&servo_pid_1);
    for(;;)
    {
        HAL_GPIO_TogglePin(LED_W_GPIO_Port, LED_W_Pin);
//        car_update(&mycar);
//        servo_pid_update(&servo_pid_1);
        //osDelay(3);
        osDelay(500);
    }

}